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function.  The  filter  computes  an  estimate  of  the  full  SWAMP  trajectory.  A  cost  function  is  provided  in  the  form 
of  a  weighted  sum  of  squares  of  the  residues  of  the  rate  gyros  and  accumulators.  Of  gradient  techniques  investi¬ 
gated  for  the  minimization  of  the  cost  function,  the  Fletcher-Powell  technique  was  found  to  be  superior  (as  mod¬ 
ified  because  of  the  extreme  sensitivity  of  the  roll-damping  coefficient),  and  only  results  for  this  technique  are 
presented. 
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PROBLEM 


Model  a  specially  instrumented,  torpedolike  Shallow  Water  Mobile  Platform 
(SWAMP)  vehicle  for  gathering  acoustic  research  data. 


RESULTS 


The  SWAMP  model  was  based  on  torpedo  motion  equations  with  six  degrees  of 
freedom.  Multiplexed  dynamic  data  employed  included  clock  time,  body  angular  rate  com¬ 
ponents,  control  surface  deflections,  propeller  rotation  speed,  vehicle  course,  and  depth. 
First-order  likelihood  identification  algorithms  were  used  as  a  guide  in  parameter  estimation. 
The  algorithms  yielded  an  estimation  of  the  state  of  the  system  by  means  of  an  extended 
“Kalman”-type  filter  as  well  as  a  weighted  quadratic  function  of  the  residues  (ie,  differences 
between  measurements  and  their  computed  values)  as  the  maximum  likelihood  cost  func¬ 
tion.  Of  gradient  techniques  investigated  for  the  minimization  of  the  cost  function,  a  modi¬ 
fied  Fletcher-Powell  technique  was  found  to  be  superior. 


RECOMMENDATIONS 


Further  work  on  post-run  analysis  should  include  analysis  of  the  data  to  estimate 
correlation  matrices;  improvement  of  the  system  model  by  identification  of  nonlinear  effects 
in  yaw,  pitch,  and  roll  coupling;  and  further  development  of  digital  recording  and  playback 
equipment  for  use  in  torpedo  guidance  and  control  development,  test,  and  evaluation. 
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1.  INTRODUCTION 


System  identification,  or  modeling,  is  one  of  the  problems  considered  by  modern 
systems  theory.  It  consists  in  determining  a  difference  or  differential  equation,  or  the  coef¬ 
ficient  parameters  of  such  an  equation,  so  that  it  describes  the  evolution  of  a  physical 
process  in  conformance  with  a  specified  criterion.  The  system  that  is  considered  in  this 
report  is  the  Shallow  Water  Mobile  Platform  (SWAMP);  its  model  is  the  torpedo  motion 
equations  with  six  degrees  of  freedom,  in  finite  difference  form,  in  which  the  hydrodynamic 
coefficients  (force  and  moment  derivatives)  are  to  be  determined.  Interest  in  the  modeling 
of  torpedoes  had  its  origin  during  World  War  II.  The  problems  that  the  Navy  encountered 
with  many  submarine-  and  aircraft-launched  weapons  inspired  research  into  torpedo  dynam¬ 
ics  and  the  establishment  of  hydrodynamic  facilities  for  the  testing  of  small-scale  torpedo 
models  and  the  measurement  of  hydrodynamic  coefficients.  Analytical  methods  based  on 
these  experimental  data  were  also  developed  for  estimating  hydrodynamic  coefficients  of 
new  torpedo  configurations.  The  mathematical  models  that  were  formulated  have  been 
employed  since  then  in  the  design  of  new  torpedoes  and  in  the  prediction  of  their  perform¬ 
ance.  Verification  of  simulation  models  has  always  been  rather  problematical.  Sea  run  data 
have  been  recorded  by  oscillograph  tracing  on  photographic  film.  Matching  these  records 
against  a  computer  model  has  been  accomplished  by  tweaking  favorite  parameters  in  the 
model  until  the  response  produced  appeared  visually  to  approximate  the  photographic 
record. 

There  was  an  analogous  development  in  aircraft  technology.  However,  new  param¬ 
eter  e  .timation  techniques  began  to  be  introduced  in  the  mid  1960s.  These  techniques, 
which  revolutionized  estimation  of  aircraft  flight  parameters,  were  made  possible  by  two 
factors:  (1)  highly  automated  data  acquisition  systems  were  becoming  standard  in  flight 
testing;  and  (2)  large  high-speed  digital  computers  were  able  to  solve  complicated  algorithms 
efficiently.  By  1973,  this  technology  was  widespread  through  the  research  field,  so  that  a 
symposium  on  parameter  estimation  techniques  and  their  application  to  aircraft  flight  test¬ 
ing  could  be  held  (reference  1). 

The  SWAMP  is  a  specially  instrumented  torpedolike  vehicle  whose  purpose  is  to 
gather  acoustic  research  data.  The  dynamics  data  of  the  vehicle  are  recorded  on  a  single 
channel  of  the  same  magnetic  tape  on  which  the  acoustic  data  are  recorded.  These  multi¬ 
plexed  data  include  a  clock  time,  body  angular  rate  components,  body  acceleration  compo¬ 
nents,  control  surface  deflections,  propeller  rotation  speed,  vehicle  course,  and  depth.  The 
rate  gyros  and  accelerometers  are  in  a  Mk  46  control  unit,  and  they  are  used  for  vehicle 
control.  The  data  were  sampled,  digitized,  and  recorded  at  intervals  of  23.04  milliseconds. 
The  first-order  maximum  likelihood  identification  algorithms  of  reference  2  (see  table  3.4-1 
below)  were  used  as  a  guide  in  the  parameter  estimation  of  this  report.  These  algorithms 
yield  an  estimation  of  the  state  of  the  system  by  means  of  an  extended  “Kalman  filter,”  as 
well  as  a  weighted  quadratic  function  of  the  residues  (ie,  differences  between  measurements 
and  their  computed  values)  as  the  maximum  likelihood  cost  function.  The  weighting  matrix 
of  the  cost  is  the  inverse  of  the  covariance  matrix  of  the  measurements.  The  filter  used  here 

1.  National  Aeronautics  and  Space  Administration  TN  D-7 647,  Parameter  Estimation  Techniques  and 
Applications  in  Aircraft  Flight  Testing,  April  1974. 

2.  Sage,  A.  P.,  and  J.  C.  Melsa,  System  Identification,  Academic  Press,  1971 . 
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Ls  not  a  true  Kalman  filter.  The  state  vector  was  partitioned ,  with  part  of  the  system  being 
filtered  by  means  of  a  constant-gain  matrix.  The  gain  matrix  was  obtained  with  some 
assumptions  about  the  noise  statistics  and  some  experimentation  with  the  filter.  The 
remainder  of  the  system  was  filtered  in  an  ad  hoc  fashion.  The  filter  computes  an  estimate 
of  the  full  SWAMP  trajectory.  The  cost  function  is  a  weighted  sum  of  squares  of  the  resi¬ 
dues  of  the  rate  gyros  and  accelerometers.  The  weights  were  chosen  according  to  a  relative 
order-of-magnitude  estimate  of  their  variances.  Various  gradient  techniques  were  investi¬ 
gated  for  the  minimization  of  the  cost  function.  The  Fletcher-Powell  technique  (reference 
2,  chapter  4)  was  found  to  be  superior  to  others  that  were  tried,  and  only  the  results  for 
this  technique  are  presented.  It  was  found  necessary  to  introduce  a  modification  of  this 
method  because  of  the  extreme  sensitivity  of  the  roll-damping  coefficient.  The  minimizing 
process  had  to  restrict  the  range  of  this  parameter  to  prevent  instability. 


2.  THE  RECURSIVE  FILTER 

As  a  model  for  the  trajectory  filter  development,  we  follow  the  treatment  by  M.  Aoki 
in  reference  3.  Suppose  the  system  to  be  estimated  has  the  evolution  equation 

-i+1  =  Ai-i  +  Bi-i  +  Ci^i  (2-1) 

where  xj  is  the  state  vector  at  the  itB  step,  Uj  is  the  control  vector  at  the  itB  step,  and  Jj  is  a 
stochastic  vector  assumed  to  be  a  zero-mean,  Gaussian  random  variable.  Aj,  Bj,  and  Cj  are 
matrices  of  appropriate  dimensions.  The  measurement  vector  y-  is  given  by 

Xi  =  Xj  +  (2.2) 

where  ^  is  the  measurement  noise  vector  assumed  to  be  also  zero-mean  Gaussian.  It  is 
assumed  that  £j  and  ?jj  are  independent  and  that  covariance  matrices  are  of  the  form 

cov(li>lj)  =Qi5ij  (2-3) 

cov  (2j,  2j)  =  Rj  6jj  (2.4) 

cov  (lj,  22j)  =  0  all  i,j  (2.5) 

where  6jj  is  the  Kronecker  delta.  The  optimal  estimate  X;  is  given  by 

*k+l  =  *k+l  +  Kk+1  (Xk+1  “  Hk+1  *k+l )  (2-6> 

*k+l  =  Ak  *k  +  Bk  — k-  (2-7> 

The  filter  gain  K|<+ j  is  defined  by 

Kk+1  =  Pk+1  Hk+1  (Hk+1  pk+l  Hk+1  +  Rk+l)_1  (2-8) 

where  P|<+]  is  the  a  priori  variance  given  by  the  evolution  equation 

Pk+1  -  Ak^kAk  +  (-k^k^k  (2-9) 

3.  Aoki,  M.,  Optimization  of  Stochastic  Systems,  Academic  Press,  1967. 


Tk  is  the  a  posteriori  variance  recursively  defined  by  ] 

rk+l  =Pk+l  "Kk+1  Hk+1  pk+l-  (2-10>  I 


If  Qj  and  Rj  are  constant  matrices  independent  of  i,  and  Aj,  Hj,  and  Cj  are  also  independent 
of  i,  the  filter  gain  reaches  a  steady-state  value  K.  Moreover,  if  Qj  =  o 2  Qj  and  Rj  =  p~ Rj, 
then  the  steady-state  value  of  Pj  is  proportional  to  a2,  say  Pj  =  o2  Pj.  We  then  have  the  filter 
gain  given  by 

Kj  =  PjHjlHjPj  H|  +  (PW)  Rj]'1  (2.1 1) 

so  that  the  steady-state  gain  depends  only  on  the  ratio  of  p and  o~.  If  this  ratio  is  chosen 
too  large,  the  filter  becomes  unstable  in  correcting  initial  errors.  A  value  was  empirically 
determined  that  resulted  in  a  stable  filter  and  a  smooth  trajectory. 


3.  APPLICATION  TO  SWAMP 

The  equations  of  motion  for  the  SWAMP  vehicle  are  standard  torpedo  motion  equa¬ 
tions  (reference  4).  They  govern  the  evolution  of  the  components  on  the  body-fixed  axes 
of  the  linear  velocity  V  and  the  angular  velocity  co 

V  =  ui+y[+wk  (3.1) 

to  =  pi+qji  +  r  k  (3.2) 

where  j,  j,  k  are  unit  vectors  in  the  direction  of  the  positive  body  x,y,z  axes.  The  evolution 
of  the  other  state  variables,  geographical  coordinates  of  the  vehicle  CB,  and  vehicle  orienta¬ 
tion  (Euler  angles  or  direction  cosines)  is  obtained  by  integration  of  the  appropriate  velocity 
components. 


For  the  purpose  of  constructing  a  manageable  filter,  we  have  partitioned  the  state 
variables.  Let  the  vector  x  be  defined  as 


x  = 


q 

w 

r 

v 

P 


(3.3) 


Then  the  equations  of  motion  give 
x  =  $  (x,  U) 


(3-4) 


where  U  is  a  “control”  vector  containing  the  longitudinal  speed  component  u,  unbalanced 
roll  torque  K0,  control  surface  deflections  5e,  6jj,  8l,  and  direction  cosines  of  the  gravita¬ 
tional  vector  G  j ,  C2,  G3.  We  employ  a  fixed  time  step  At  and  define 


4.  Naval  Ordnance  Test  Station  (Pasadena)  NAVORD  report  2090,  Motion  Equations  for  Torpedoes, 
by  L.  Lopes,  12  February  1954. 
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Then,  approximately, 


xjc+i  =  x.k  +  4  (Xk>  Uk^  At-  (3.6) 

Filter  gain  will  be  determined  from  the  linearized  equation,  linearized  about  x  =  0,  U  =  UQ, 
corresponding  to  straight,  level  flight  at  constant  speed. 

-k+1  =  ^-k  +  ®?^k  (3.7) 


where 

A  =  I  +  (a,  Uq)  At  (3.8) 

B  =  4>u  (o,U0)At.  (3.9) 


The  measurement  vector  is  taken  to  be  body  rates  as  measured  by  rate  gyros,  so  that 


where 

[1  0  0  0  O' 
0  0  10  0 
00001. 


(3.10) 


(3.11) 


Comparing  (3.7)  with  (2.1),  we  introduce  the  noise  vector  with  the  assumption  that  Cj  is  a 
constant  diagonal  matrix  and  Qj  =  <j2  I.  Introducing  measurement  noise  in  (3.10),  we 
assume  Rj  to  be  a  constant-diagonal  matrix.  Equations  (2.9)  and  (2.10)  are  then  iterated  to 
obtain  a  steady-state  constant  filter  gain  K.  The  filter  is  then  given  the  form  of  the  extended 
Kalman  filter  for  nonlinear  systems 


*k+l  =Xk  +  $(Xk’i!k)At 


(3.12) 


*k+l  "*k+l 


+  K(xk-Hxk+1). 


(3.13) 


The  operator  H  will  in  fact  be  a  matrix  with  differential  operator  elements  accounting  for 
the  second-order  rate  gyro  response  and  the  limited  output  of  the  gyros. 


Let  us  consider  the  “control”  vector  Uk  in  more  detail.  The  axial  speed  component 
u  is  governed  by  the  longitudinal  force  equation  which  includes  propeller  thrust,  vehicle 
drag,  the  axial  component  of  the  net  buoyancy  force,  and  centrifugal  force  terms.  The  pro¬ 
peller  thrust  is  obtained  from  the  empirically  determined  characteristics  of  the  RETORC  1 
propellers,  and  is  a  function  of  u  and  the  propeller  rotation  speed.  The  vehicle  axial  drag  is 
a  function  of  u.  The  drag  coefficient  of  the  RETORC  was  modified  to  give  the  correct  dive 
rate  to  the  SWAMP  vehicle.  The  measured  control-surface  deflections  were  fed  directly  into 
the  motion  equations.  Random  noise  in  these  measurements  will  be  filtered  by  the  motion 


equations.  Vehicle  orientation  may  be  specified  as  in  reference  4  by  the  Euler  angles  \ j/,6, 
ip  (course,  pitch,  and  roll),  or  by  the  direction  cosines  of  the  vectors Iq  and  where  is 
the  horizontal  vector  in  the  direction  of  the  gravitational  force.  From  appendix  E  of  refer¬ 
ence  2,  we  have 

i0  =H1i+H2i  +  H3k 

k0  =  Gii  +  G2i+G3k 

where 

Hj  =  cos  it  cos  6 

H2  =  cos  \p  sin  6  sin  <p  -  sin  cos  <p 
H3  =  sin  \p  sin  <p  +  cos  \p  sin  8  cos  i p. 

Gj  =  -sin  8 
G2  =  cos  8  sin  ip 

G3  =  cos  8  cos  ip.  (3.14) 

The  Euler  angles  may  be  recovered  from  these  direction  cosines  as  follows: 
ip  =  tan~l  (G2/G3) 

e  =tan-l(-G,  ) 

\p  =  tan"*  [cosd(-H2cosip+H3sinp)/Hj].  (3.15) 

To  emphasize  the  direction  cosine  formulation,  we  write  jo  -  H,  k0  =  G.  The  evolution 
equations  for  H  and  G  are 

H  =  H  X  w 

G  =  GXw  (3.16) 

which  express  the  fact  that  they  are  fixed  vectors  relative  to  geographical  coordinates.  A 
predictor-corrector  approach  is  employed  to  estimate  these  vectors  for  use  in  the  filter.  The 
predictor  is  obtained  by  discretizing  the  evolution  equations  (3.16). 


Hk+i  =Hk  +  Hk  X  wk  At  (3.17) 

Qji+1  =  Qk  +  Gk  X  wk  At.  (3.18) 

The  correction  is  obtained  by  compensating  the  accelerometer  outputs  for  vehicle  motion. 
The  acceleration  measured  by  the  instruments,  expressed  in  gravity  units,  is 

lrk0_(i  +  y  Xv  +  wXrj  +  wX  uX  la)/g  (3.19) 

where  ra  is  the  (fixed)  vector  from  the  origin  of  body  coordinates  to  the  accelerometer,  and 
g  is  the  acceleration  of  gravity.  Compensation  of  the  accelerometer  by  addition  of  the 
acceleration  arising  from  vehicle  motion  estimated  by  the  filter  yields  an  estimate  of  the 
vector  kQ  at  the  k^1  step,  Gk.  This  is  a  noisy  estimate.  Correction  of  (3.18)  is  affected  by 


rotation  of  Gk+j  through  a  fraction  of  the  angular  distance  toward  Gk+j  .  Let  Qk  =  f  Gk 
X  Gk.  Then  the  correction  to  Gk+j  is 

Gk+1  x  =  ~ft^k+l  "  (§k+l  •  Qk+1)  Gk+1 1 ,  (3.20) 

since  Gk+j  is  a  unit  vector.  (Hk  and  Gk  are  periodically  renormalized.)  Correction  for 
(3.17)  is  obtained  with  use  of  the  course  gyro.  Let  the  course  gyro  value  at  the  k^1  sample 
be  denoted  by  t!/k,  and  let  ek  =  hj(^k  -  ^k).  Then  the  correction  added  to  (3.17)  is  a  rota¬ 
tion  about  Gk+i  of  Hk+i  through  the  angle  ek.  In  addition,  a  correction  proportional  to 
Gk+i  •  Hk+j  is  applied  to  mair'ain  orthogonality  of  Gk  and  Hk.  Special  treatment  must 
be  given  to  cases  where  i//k  is  in  the  neighborhood  of  ±  180  degrees,  since  incorrect  course, 
indication  occurs  there  because  of  potentiometer  limitations. 

Trajectory  increments  are  now  obtained  with  the  formula 

xk+l=xk+ik‘HkAT 
yk+l  "  Xk  +  — k  *  (Gk  x  Hk)  Ar 
zk+l  =  zk  +^k  *  Sk  At. 

A  correction  to  zk+j  is  made  by  fractional  movement  toward  the  (noisy)  hydrostat  depth 
indication.  Unbalanced  roll  torque  is  controlled  with  the  roll-rate  error.  A  complete  speci¬ 
fication  of  the  filter  equations  is  presented  in  the  appendix. 


4.  SWAMP  SEA  RUN  RESULTS 

The  filter  was  originally  run  by  means  of  hydrodynamic  coefficient  data  from  refer¬ 
ence  5  modified  for  the  greater  length  of  the  SWAMP  vehicle.  There  have  been  12  success¬ 
ful  SWAMP  runs.  Two  of  these  were  chosen  to  provide  data  for  testing  parameter  estimation 
techniques:  run  no.  4  and  run  no.  6.  Figures  4-1  and  4-2  show  their  respective  estimated 
trajectories.  The  figures  show  the  x-y  plots,  with  run-time  tags  at  10-second  intervals,  and 
depth  versus  time. 

Intervals  were  selected  from  these  runs  in  which  significant  maneuvers  occurred. 
From  run  no.  4,  the  following  intervals  were  chosen:  (4A)  from  41  to  61  seconds,  in  which 
the  vehicle,  while  running  straight  at  constant  depth,  started  turning  at  a  rate  of  4  degrees 
per  second;  (4B)  from  151  to  171  seconds,  in  which  the  vehicle  started  to  dive  at  a  10-degree 
down  angle  from  a  condition  of  running  at  constant  depth  and  turning  at  4  degrees  per  sec¬ 
ond;  (4C)  from  247  to  267  seconds,  in  which  the  vehicle  pulled  out  of  the  dive  and  resumed 
circling  at  constant  depth.  From  run  no.  6,  the  following  intervals  were  chosen:  (6 A)  from 
51  to  71  seconds,  in  which  the  SWAMP  pulls  out  of  its  initial  45-degree  dive  angle  to  run  at 
constant  depth,  then  initiates  a  snake  trajectory  about  a  prescribed  course;  and  (6B)  from 
136  to  1 56  seconds,  in  which  the  vehicle  terminates  the  snake,  executes  a  90-degree  turn, 
and  commences  a  straight  run  on  rate  gyro  control. 


5.  David  W.  Taylor  Naval  Ship  Research  and  Development  Center  report  276-H-01 ,  Model  Investigation  of 
the  Submerged  Stability  and  Control  Characteristics  of  RETORC  I,  by  J.  H.  Wolff,  August  1968. 
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5.  COST  FUNCTION  AND  ITS  MINIMIZATION 


The  cost  function  was  computed  from  the  residues  resulting  from  running  the  filter 
for  one  of  the  selected  intervals.  The  filter  was  initialized  1  second  before  the  chosen  inter¬ 
val,  using  initial  conditions  given  by  the  original  run  of  the  complete  trajectory.  The  filter 
was  then  run  for  that  1  second,  before  starting  to  compute  the  cost  function,  to  minimize 
the  effects  of  initial  condition  transients.  The  cost  function  is  a  weighted  sum  of  the  squares 
of  the  rate  gyro  and  the  accelerometer  residues.  Let  Ep,  Eq,  and  Er  denote  the  root  mean 
square  (RMS)  values  of  the  roll,  pitch,  and  yaw  rate  gyro  residues,  respectively.  Let  Ex,  Ey, 
and  Ez  denote  the  RMS  values  of  the  accelerometer  residues  for  the  x,  y,  z  axes,  respectively. 
The  cost  function  was  chosen  to  be 

J(a)  =  0.1  E2  +  E^  +  E^+  1000  El+  1000  E%  +  100  e\. 


The  weights  were  chosen  so  that  the  contribution  of  each  of  the  RMS  residues  would 
have  the  same  order  of  magnitude  as  the  others,  based  on  the  original  filter  run  data.  The 
vector  a  was  chosen  to  have  the  following  components  (see  appendix): 


—  (LTa>  ^Se»  ^WB’  ^YB>  ^qB’  rk>  ^p>  ^eo>  ^ro^ 


where  5eo  and  5ro  are  biases  in  the  control-surface  measurements.  The  coefficient  Z^?g  is 
not  included  because  it  is  immaterial,  as  far  as  the  linearized  equations  of  motion  are  con¬ 
cerned,  whether  lift  is  ascribed  to  the  body  or  to  the  tail.  Consequently,  Z^yg  retains  its 
original  estimated  value  throughout,  as  do  all  the  other  parameters  in  the  equations  of 
motion. 


Various  gradient  methods  (eg,  steepest  descent)  were  tried  in  the  minimization  of 
the  cost  function  before  it  was  determined  that  the  Fletcher-Powell  method  would  work 
satisfactorily. 


To  implement  the  Fletcher-Powell  method,  the  gradient  of  J(a)  must  be  computed. 
For  this  purpose,  small  fixed  increments  hi  of  the  components  of  a  were  chosen  and  the 
central  difference  quotient  was  used  as  an  approximate  gradient. 

gi(a)=  fJ(a  +  hiei)-(a-hiei)]/(2hi)  (5.1) 

where  e^j  is  the  unit  vector  for  the  i^  coordinate  in  a  space.  A  sequence  of  matrices  Gn  is 
computed  which  converges  to  the  inverse  of  the  Hessian  matrix  (reference  2,  p  90).  At  each 
step,  a  search  vector 

Sn  =  -Gng(an)  (5.2) 

is  computed.  It  is  then  required  that  the  value  \x  =  p*,  which  minimizes  J(an  +  pSn),  be 
established.  A  quadratic  approximation  was  found  to  be  adequate,  using  the  points  J(an), 
J(an  +  Sn),  and  J(an  +  0.5  Sn).  But  in  any  event,  the  range  of  Kp  was  restricted  to  be 
between  -0.01  and  -0.00004  to  prevent  instability  in  the  filter.  The  new  parameter  vector 
an+l  =  an  +  Aan,  where  Aan  =  /i*Sn  is  computed,  and  then  the  matrix  Gn+*  =  Gn  +  AGn, 
where 


- - -  -v  » j\(<,  V 


AG„  _  Aan(Aan)T  GnAg,n(Agn)T  Gn 
(Aa11)^  Aa°  (Ag0)^  GnA£n 

and  where  Agn  =  g  (an+  ^ )  -£(an).  This  procedure  is  iterated  until  some  criterion  is  satis¬ 
fied.  In  the  runs  made  for  this  report,  G°  was  the  identity  matrix  and  the  effective  stopping 
criterion  was  either  a  specified  maximum  number  of  iterations  or  a  maximum  computer 
time  (1  hour).  A  flowchart  for  the  Fletcher-Power  method  is  given  ‘  figure  5-1. 
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6.  RESULTS 

In  all  the  runs,  the  initial  parameter  set  was  the  original  set  which  used  the  hydrody¬ 
namic  data  of  reference  5  modified  for  the  SWAMP  length.  Figure  6-1  shows  the  cost  func¬ 
tion  as  a  function  of  the  iteration  number  for  runs  4A,  4B,  4C,  6A,  and  6B.  The  process  is 
evidently  converging  for  each  of  the  runs.  The  original  parameter  set  and  the  final  sets  for 
each  of  the  runs  are  presented  in  table  6-1 .  The  most  notable  item  in  the  results  is  the  large 
values  for  the  moment  coefficients  M\yg  and  Mqg.  The  variation  of  the  other  parameters 
from  the  original  estimates  was  not  unexpected,  since  they  lie  within  tolerances  usually 
ascribed  to  such  data.  The  averages  of  the  final  values  are  also  given.  To  provide  a  perspec¬ 
tive  of  the  residues  of  the  individual  rate  gyros  and  accelerometers,  the  RMS  values  for  the 
original  and  final  parameter  sets  are  tabulated  in  table  6-2,  along  with  residues  derived  by 
means  of  the  average  of  the  final  parameter  sets.  A  comparison  of  the  measured  instrument 
data  and  that  computed  by  the  filter  is  shown  in  figures  6-2  (a,  b,  c,  d,  e,  0  to  6-6  (a,  b,  c, 
d,  e,  f).  The  dotted  lines  represent  the  measured  data  and  the  solid  lines  are  the  filter  output 


^eo 


Run 

l't 

ZS 

mwb 

MqB 

ZqB 

rk 

(deg) 

^ro 

4A 

1.745 

-0.956 

1.895 

-0.890 

-0.123 

0.337 

-0.00242 

0.48 

-0.78 

4B 

1.625 

-0.893 

1.760 

-0.678 

-0.115 

0.381 

-0.00156 

0.37 

-0.40 

4C 

1.633 

-0.914 

1.682 

-0.653 

-0.043 

0.288 

-0.00084 

0.22 

-0.00 

6A 

1.878 

-0.885 

1.814 

-0.517 

-0.417 

0.320 

-0.00144 

0.31 

-0.70 

6B 

1.698 

-1.000 

1.871 

-0.828 

-0.231 

0.295 

-0.00151 

0.29 

-0.65 

Avg 

1.716 

-0.930 

1.804 

-0.713 

-0.186 

0.324 

-0.00155 

0.33 

-0.51 

Orig 

1.887 

0.940 

1.300 

-0.130 

-0.200 

0.400 

-0.00100 

0.00 

0.00 

Table  6-1.  Parameter  sets  (final). 
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>  vrtV-V^— w-w-rcw «~3?s*£w ~  <>^**^ ^£S£ 


Run 

EP 

Eq 

Er 

Ex 

Ey 

Ez 

4A 

Final 

0.3378 

0.1000 

0.1337 

0.00180 

0  00610 

0.02285 

Orig 

0.498o 

0.2941 

0.2750 

0.00209 

0.00456 

0.01355 

Avg 

0.3970 

0.1026 

0.1540 

0.00180 

0.00553 

0.02201 

4B 

Final 

0.5021 

0.1404 

0.1580 

0.00324 

0.00581 

0.01927 

Orig 

0.8730 

0.3038 

0.2204 

0.00419 

0.00720 

0.02001 

Avg 

0.6466 

0.1222 

0.1606 

0.00320 

0.00662 

0.01798 

4C 

Final 

1.297 

0.1807 

0.2230 

0.00404 

0.01147 

0.02164 

Orig 

1.698 

0.4410 

0.2450 

0.00713 

0.01625 

0.02389 

Avg 

1.439 

0.1686 

0.1798 

0.00413 

0.01267 

0.02123 

6A 

Final 

0.6085 

0.1606 

0.1610 

0.00400 

0.00536 

0.01713 

Orig 

0.7030 

0.4129 

0.3104 

0.00446 

0.00620 

0.01454 

Avg 

0.6125 

0.1583 

0.1560 

0.00540 

0.00761 

0.01849 

6B 

Final 

0.8063 

0.1213 

0.1502 

0.00251 

0.00969 

0.01868 

Orig 

0.9362 

0.3130 

0.3201 

0.00202 

0.00942 

0.01671 

Avg 

0.8262 

0.1259 

0.1629 

0.00256 

0.00944 

0.01909 

Table  6-2.  RMS  instrument  residues. 
(Rate  gyros  deg/s,  acceleration,  g) 
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Figure  64.  Swamp  run  no.  4. 
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Figure  6-6.  Continued 


7.  CONCLUSIONS  AND  RECOMMENDATIONS 


1 .  The  implementation  of  the  Fletcher-Powell  method  used  here  is  a  practical  tech¬ 
nique  for  estimating  hydrodynamic  coefficients  from  sea-run  data. 

2.  Further  work  on  post-run  analysis  should  include: 

a.  Analysis  of  the  data  to  estimate  correlation  matrices.  This  is  necessary  if 
optimal  Kalman  filters  and  maximum-likelihood  identification  are  to  be 
implemented. 

b.  Improvement  of  the  system  model  by  identification  of  nonlinear  effects  in 
yaw,  pitch,  and  roll  coupling. 

c.  Further  development  of  digital  recording  and  playback  equipment  for  use  in 
torpedo  guidance  and  control  development,  test,  and  evaluation. 
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APPENDIX:  FILTER  EQUATIONS 


The  instruments  used  in  the  measurements  are  the  same  as  in  the  control  system  of 
the  Mk  46  Mod  1  torpedo.  The  rate  gyros  and  accelerometers  have  a  second-order  response 
characteristic  with  a  natural  frequency  of  about  27  Hz  and  a  damping  ratio  of  about  0.7. 

Let  the  operator  L  be  identified  by 

L  =  1  +  (2f/o>0)  D  +  (1/<o2)D2  (A.l) 

where  D  =  d/dt.  Then  the  rate  gyro  equations  are 

(a)  Lpg  =  p 

(b)  Lpg  =  q 

(c)  Lrg  =r  (A. 2) 

where  pg,  qg,  and  rg  are,  respectively,  the  roll,  pitch,  and  yaw  rate  gyro  outputs  and  the  caret 
indicates  that  the  state  variable  input  is  limited  to  1  radian  per  second. 

The  accelerometers  have  a  similar  response.  Let  Ax,  Ay,  and  Az  be  the  x,  y,  and  z 
accelerometer  outputs,  respectively.  Then 

(a)  LAX  =  ax 

(b)  LAy  =  ay 

(c)  LAZ  =  az  (A.3) 

where 

(a)  ax  =  Gj  -  [u  +  wq  - vr  +  qzx  - ryx  +  p(qyx  +  rzx)  +  (q2  +  r2)Xx] /g 

(b)  ay  =  G2  -  [v  +  ur  =  wp  +  rxy  -  pZy  +  q(rZy  +  pzy)  -  (r2  +  p2)yy ]  /g 

(c)  z  =G3-[w  +  vp-uq  +  py2-qxz  +  z(pxz  +  qyz)-(p2  +  q2)Zz]/g  (A.4) 

and  where  (xx,  yx,  zx),  (xy,  yy,  Zy),  and  (xz,  yz,  zz)  are  the  respective  coordinates  of  the  x, 
y,  and  z  accelerometers. 

The  motion  equations  for  the  first -stage  predictor  are 

(a)  m^u  =  THRUST  -  DRAG  +  (W  -  B)  Gj  -  mj(wq  -  vr)  +  mzQpr 

(b)  mTw  =  Zqg  uq  +  ZwBuw  +  ZwD  |w|  w  +  ZT  +  (W  -  B)  G3 

(c)  mTv  =  YmBur  +  Yyguv  +  YvD  |v|  v  +  Yjy  +  YTL  +  (W  -  B)  G2  +  mzG(p  -  qr) 

(d)  Jxp  =  Kp  up  +  rk(YTU  -  Ytl)  +  mzQ(v  +  ur  -  wp)  -  Wzf^  +  Ko 

(e)  Jyq  =  MwBuw  +  MqBuq  -  xT  Zj  -  W(xqG3  -  ZqG  j  )  -  mzQ  (u  +  wq  -  vr) 

+  (Jy-Jx)Pr 

(0  Jzr  =  NvBuv  +  NrBur  +  Xj(Ytu  +  YTL)  +  WXQG2  -  (Jy  -  Jx)pq  (A.5) 

where 

ZqB  =  ^2  A£ZqB  +  mL’  ZwB  =  ^2AZwB» 
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ZWD  =  ~p^2  A(2DC>  ^wB  =  p!2  a^wB> 

MqB  =P/2AC2MqB,  YrB  =  -ZqB,  YyB  =  ZwB, 

YyD  ~  ^w£),  NvB  MwB,  NfB  -  MqB, 

Kp  =  p/2  At2Kp. 

THRUST  is  a  homogeneous  quadratic  function  of  u  and  RPS,  propeller  rotational  speed, 
that  was  fitted  to  empirical  data  for  the  RETORC  I  propellers. 

THRUST  =  -0.0136  u2  -  0.659u  •  RPS  +  2.07  RPS2  (A.6 

DRAG  =  p/2  A  (CD  u2  +  0.676u  -  3.38),  u  >  4.  (A.7 

The  drag  coefficient  Cp  was  adjusted  to  give  a  speed  consistent  with  the  observed  dive  rate 
of  SWAMP. 

The  tail  lift  terms  are  given  by  reference  6. 

f_n/0  AT'  /.  i2  4-  (L  A  A  i  l/v I  ac\° 


-p/2  ALja  wj-y  u2  +  6.44  Wy  ,  |aj|<40° 

-p/2  A  ( 1 .4)  w-p \  /u2  +  Wy  ,  |aj|  >  40° 


where  wT  =  w  -  xTq  -  Cgu5e,  Cg  =  Zg  'L/p^ 
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YTU“ 


YTL" 


-p/4  ALja  vtu  ^ 

^u2  +  6.44  Vy|j, 

I/3tuI<40° 

-p/4  A(1 .4)  vj\jy 

Ju2  +  v^y  , 

l^ul>40° 

(A.9) 

-p/4  ALja  vTL  y 

/u2  +  6.44  v2l  , 

|0tlI<4O° 

-p/4  A(  1 .4)  vTL  y 

,/u2  +  vtl> 

I^TuI  ^ 

(A.  10) 

where 


vTU  =  v  +  xTr  +  Cgu5u 
vTl  =  v  +  xjr  +  CguSp. 

The  evolution  equations  for  the  direction  cosines  are 

(a)  G  j  =  rG2  -  qGj 

(b)  G2  =  PG3  -  rG  j 

(c)  63  =  qG  j  -  PG2 
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(d)  Hj=rH2-qH3 

(e)  H2  =  pH3-rH1 

(f)  fi3  =  qH1=pH2. 

The  equations  for  the  geographic  x-y  coordinates  and  depth  z  are 

(a)  x  =  uHj  +  vH2  +  wH3 

(b)  y  =  uFj  +  vF2  +  wF3 

(c)  z  =  uGj  +  vG2  +  wG3 

where  F  =  G  X  H. 


(A.l  1) 


(A.  12) 


The  corrector  equation  for  the  vector  (3.3)  is  in  accordance  with  (3.13).  The  vector 
Hxjf+i  is  the  output  of  equations  (A.2)  at  the  k^1  step.  When  a  rate  gyro  measurement  sat¬ 
urates,  the  corresponding  correction  is  set  to  zero. 

The  direction  cosine  vector  G  is  not  corrected  for  the  first  10  seconds  after  water 
entry  to  allow  time  for  the  initial  transient  to  pass.  After  this  time,  the  body  acceleration 
terms  of  equations  (A.4)  are  used  to  compensate  the  measured  accelerations  and  thereby 
provide  an  indication  of  the  gravitational  vector  Gj.  The  vector  G  is  then  corrected  accord¬ 
ing  to  the  equation 


-k+1  =Qk  +  01  ^Ik'Sk) 
where  Gk  is  the  predicted  value  using  (A.l  1). 

The  H  vector  is  corrected  by  the  equation 


(A.l  3) 


Hk+1  =  -k+1  +a^k+l  x  3k+l  "0£k+l  (A- 14) 

where 

a  =  0.1  (*k+1  -*!) 

P  =  0-l  (Qk+1  '  Sk+1^’ 

4,jc+l  being  the  course  angle  determined  by  Gk+j  and  Hk+j ,  and  the  current  course 
gyro  measurement.  When  the  course  is  in  the  neighborhood  of  ±180°,  the  gyro  does  not 
give  a  correct  course  measurement.  During  this  period,  a  is  set  at  zero. 


Depth  is  corrected  according  to  the  hydrostat  measurement  by 
zk+l  =Zk+l  +  0-01  (zI-zk+l) 


(A. 15) 


where  zj  is  the  current  hydrostat  indication  of  depth  and  zk+j  is  the  depth  predicted  by 
(A.  12c). 

Unbalanced  roll  torque  is  controlled  by  the  difference  between  measured  and  esti¬ 
mated  roll  rate. 


^o,k+l  ^o,k  +  ^-5  (PG  ~ Pgk+1 ) 

where  pq  is  the  current  roll  rate  measurement,  and  P„i.+i  is  the  current  computed  rate  gyro 
output.  8  1 
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